#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdint.h"
#include <stdarg.h>
#include <math.h>

// RT-Thread 头文件
#include <rtthread.h>
#include <rthw.h>
#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include <rtdevice.h>
#include <board.h>

#include "rt_thread_demo.h"
#include "../hardware/PWM/fly_pwm.h"                    // PWM
#include "../hardware/Flight_control/fly_control.h"     // 飞控
#include "../applications/De_bug_mouse/de_bug_mouse.h"  // 相关辅助函数

/**
 * @brief  飞控相关外设初始化状态标志
 * @note   每一位代表一个外设的初始化状态，1为成功，0为失败
 *         0x01 - UART2 (openmv/k230)
 *         0x02 - UART3 (ms53l)
 *         0x04 - UART4 (remote/debug)
 *         0x08 - UART5 (JY61P gyro)
 *         0x10 - IIC1 (BMP280)
 *         0x20 - TIM8 (PWM)
 *         0x40 - UART3 (MP3)
 *         0x80 - TIM4 (PWM-RGB)   
 */
uint8_t fly_control_init_status = 0x00; // 飞控相关外设初始化状态标志

#define MAX_HEIGHT_STEP 200 // 单位mm，每周期最大允许高度变化（激光防止跳变）

static float last_height = 0;  // 上次融合高度值
static int first_height_flag = 1;   // 是否为第一次高度融合，1表示第一次，0表示非第一次

// AT指令相关全局变量
static volatile int at_flag = 0;    // AT指令标志，1表示有新指令
static volatile at_state_t at_state = AT_STATE_NONE;    // AT指令状态
static volatile int at_pwm_val[4] = {0}; // 支持4通道PWM
int fly_unlock_flag = 0; // 解锁标志，1表示已解锁，0表示未解锁
float User_Speed_Base = 0;// 用户速度基准值，初始化时设置为0

static volatile int user_ad[3] = {0}; // 三路AD
static volatile int user_key = 0;     // 按键状态

/*支持指令:
 * AT                      基础AT指令，返回OK
 * AT+PWM=xxx,xxx,xxx,xxx  设置4通道PWM
 * AT+TAKEOFF              起飞指令
 * AT+STOP                 停止指令
 * AT+UNLOCK               解锁指令
 * @[数据]\r\n             遥控器指令格式
*/

/*遥控器指令数组*/
uint8_t rx_1_data[30];
uint8_t rx_1_flag;
uint8_t rx_1_num;
uint8_t rx_1_buf[1];
uint8_t rx_4_data[50];
uint8_t rx_4_flag;
uint8_t rx_4_num;
uint8_t rx_4_buf[1];
uint8_t rx_5_data[30];
uint8_t rx_5_flag;
uint8_t rx_5_num;
uint8_t rx_5_buf[1];
uint8_t speed_updata_flag;


/****************************** Check_Init BEGIN *******************************
 * 函    数：Check_Init
 * 功    能：根据enable_mask检测飞控相关外设的初始化状态
 * 参    数：enable_mask - 需要检测的外设位掩码，1表示检测，0表示不检测
 * 返 回 值：无
 * 注意事项：如有需检测的外设初始化失败，打印调试信息并进入死循环
 ******************************************************************************/
static uint8_t Check_Init(uint8_t enable_mask)
{
    struct {
        uint8_t mask;
        const char* name;
    } devs[] = {
        {0x01, "UART2 (openmv/k230)"},
        {0x02, "UART3 (ms53l)"},
        {0x04, "UART4 (remote/debug)"},
        {0x08, "UART5 (JY61P gyro)"},
        {0x10, "IIC1 (BMP280)"},
        {0x20, "TIM8 (PWM)"},
        {0x40, "UART3 (MP3)"},
        {0x80, "TIM4 (PWM-RGB)"},
    };

    int all_ok = 1;
    for (unsigned i = 0; i < sizeof(devs)/sizeof(devs[0]); i++)
    {
        if ((enable_mask & devs[i].mask) == 0)
            continue; // 不检测该外设

        if ((fly_control_init_status & devs[i].mask) == 0)
        {
            Serial_printf(SERIAL1,"%s init failed!\n", devs[i].name);
            all_ok = 0;
        }
    }
    if (!all_ok)
    {
        Serial_printf(SERIAL1,"Fly control init failed, system halt!\n");
        return RT_ERROR;
    }
    return  RT_EOK;
}
/****************************** Check_Init END *********************************/

/****************************** fly_control_init BEGIN *************************
 * 函    数：fly_control_init
 * 功    能：初始化飞控相关外设，检测各外设初始化状态
 * 参    数：enable_mask - 需要检测的外设位掩码，1表示检测，0表示不检测
 * 返 回 值：RT_EOK - 所有需检测的外设初始化成功
 * 注意事项：其它外设初始化时需置对应标志位
 ******************************************************************************/
int fly_control_init(uint8_t enable_mask)
{
    if(Check_Init(enable_mask) == RT_EOK)return RT_EOK;
    return RT_ERROR;
}
/****************************** fly_control_init END ***************************/

/****************************** fly_fusion_height BEGIN ************************
 * 函    数：fly_fusion_height
 * 功    能：融合激光测距和气压计高度，防止高度突变
 * 参    数：laser_mm    —— 激光测距高度（单位mm）
 *           pressure_pa  —— 气压计压力值（单位Pa）
 * 返 回 值：融合后的高度（单位mm）
 * 注意事项：激光测距有效范围为0~2000mm，超出则用气压计估算
 ******************************************************************************/
float fly_fusion_height(float laser_mm, float pressure_pa)
{
    float new_height = 0;

    // 激光测距有效范围判断（0<laser_mm<2000为有效）
    if(laser_mm > 0 && laser_mm < 2000)
    {
        new_height = laser_mm;
    }
    else
    {
        // 超出激光量程，使用气压计估算高度
        float h = (1 - powf(pressure_pa/101325.0f, 0.1903f)) * 44330.0f * 1000.0f;
        new_height = h;
    }

    // 限幅滤波，防止高度突变
    if(first_height_flag)
    {
        last_height = new_height;
        first_height_flag = 0;
        return new_height;
    }
    else if(fabsf(new_height - last_height) > MAX_HEIGHT_STEP)
    {
        // 跳变过大，丢弃本次数据
        return last_height;
    }
    else
    {
        last_height = new_height;
        return new_height;
    }
}
/****************************** fly_fusion_height END **************************/

/****************************** at_parse BEGIN *********************************
 * 函    数：at_parse
 * 功    能：解析AT指令，设置指令标志和状态
 * 参    数：cmd —— 接收到的AT指令字符串
 * 返 回 值：无
 * 注意事项：支持AT、AT+PWM=xxx,xxx,xxx,xxx、AT+TAKEOFF、AT+STOP等指令
 ******************************************************************************/
void at_parse(const char* cmd)
{
    // 1. 查找起始符 '@'
    const char* start = strchr(cmd, '@');
    if (!start) return;  // 未找到起始符直接返回
    start++;  // 跳过 '@'，指向数据起始位置

    // 2. 查找结束符 "\r\n"
    const char* end = strstr(start, "\r\n");
    if (!end) return;  // 未找到结束符直接返回
    end += 2;  // 包含 "\r\n" 的结束位置（关键修改点）

    // 3. 计算数据长度（含 "\r\n"）
    size_t data_len = end - start;  // 包含结尾的 "\r\n"

    // 4. 安全拷贝（确保不越界）
    const size_t max_copy = sizeof(rx_4_data);
    if (data_len > max_copy) {
        // 超长时截断，但优先保留结尾的 "\r\n"
        size_t copy_len = max_copy;
        if (max_copy >= 2) {
            // 确保最后两字节是 "\r\n"
            memcpy(rx_4_data, end - max_copy, max_copy);
            rx_4_data[max_copy - 2] = '\r';
            rx_4_data[max_copy - 1] = '\n';
        } else {
            memcpy(rx_4_data, start, max_copy);
        }
    } else {
        // 正常拷贝（含 "\r\n"）
        memcpy(rx_4_data, start, data_len);
    }

    // 5. 设置控制状态标志
    at_state = AT_STATE_CONTROL;
    at_flag = 1;  // 设置AT指令标志

    return;


    //AT指令部分：
    // 拷贝并去除结尾回车换行
    char buf[64];
    strncpy(buf, cmd, sizeof(buf)-1);
    buf[sizeof(buf)-1] = 0;
    int len = strlen(buf);
    while(len > 0 && (buf[len-1] == '\n' || buf[len-1] == '\r')) buf[--len] = 0;

    // 检查是否为油门指令格式（0x55,ad1,ad2,ad3,key）
    if(strcmp(buf, "AT+STOP") == 0)
    {
        at_state = AT_STATE_STOP;
        fly_pwm_set(0,1000,1000,1000,1000); // 停止所有PWM输出
        Serial_printf(SERIAL4, "OK\r\n");
    }
    int head, ad1, ad2, ad3, key;
    if(sscanf(buf, "%d%d%d%d%d", &head, &ad1, &ad2, &ad3, &key) == 5 && head == 0x55)
    {
        user_ad[0] = ad1;
        user_ad[1] = ad2;
        user_ad[2] = ad3;
        user_key = key;
        at_state = AT_STATE_GAS_PEDAL;
        at_flag = 1;
        return;
    }
    if(strcmp(buf, "AT") == 0)
    {
        at_state = AT_STATE_NONE;
        Serial_printf(SERIAL4, "OK\r\n");
    }
    else if(strncmp(buf, "AT+UNLOCK", 9) == 0)
    {
        at_state = AT_STATE_UNLOCK;
        Serial_printf(SERIAL4, "OK\r\n");
    }
    else if(strncmp(buf, "AT+PWM=", 7) == 0)
    {
        int pwm[4] = {0};
        int n = sscanf(buf + 7, "%d,%d,%d,%d", &pwm[0], &pwm[1], &pwm[2], &pwm[3]);
        if(n == 4)
        {
            for(int i=0;i<4;i++)
                at_pwm_val[i] = pwm[i];
            at_state = AT_STATE_PWM;
            Serial_printf(SERIAL4, "OK\r\n");
        }
        else
        {
            at_state = AT_STATE_NONE;
            Serial_printf(SERIAL4, "ERROR\r\n");
        }
    }
    else if(strcmp(buf, "AT+TAKEOFF") == 0)
    {
        at_state = AT_STATE_TAKEOFF;
        Serial_printf(SERIAL4, "OK\r\n");
    }
    else
    {
        at_state = AT_STATE_NONE;
        Serial_printf(SERIAL4, "ERROR\r\n");
    }
}
/****************************** at_parse END ***********************************/

/****************************** at_get_flag BEGIN ******************************
 * 函    数：at_get_flag
 * 功    能：获取AT指令标志
 * 参    数：无
 * 返 回 值：1-有新AT指令，0-无
 * 注意事项：配合at_get_state、at_clear_flag使用
 ******************************************************************************/
int at_get_flag(void)
{
    return at_flag;
}
/****************************** at_get_flag END ********************************/

/****************************** at_get_state BEGIN *****************************
 * 函    数：at_get_state
 * 功    能：获取当前AT指令状态
 * 参    数：无
 * 返 回 值：at_state_t 枚举值
 * 注意事项：配合at_get_flag、at_clear_flag使用
 ******************************************************************************/
at_state_t at_get_state(void)
{
    return at_state;
}
/****************************** at_get_state END *******************************/

/****************************** at_get_pwm BEGIN *******************************
 * 函    数：at_get_pwm
 * 功    能：获取AT+PWM指令中的PWM值
 * 参    数：idx —— 通道号（0~3）
 * 返 回 值：PWM值（int型）
 * 注意事项：仅在AT_STATE_PWM状态下有效
 ******************************************************************************/
int at_get_pwm(int idx)
{
    if(idx < 0 || idx > 3) return 0;
    return at_pwm_val[idx];
}
/****************************** at_get_pwm END *********************************/

/****************************** at_clear_flag BEGIN ****************************
 * 函    数：at_clear_flag
 * 功    能：清除AT指令标志和状态
 * 参    数：无
 * 返 回 值：无
 * 注意事项：处理完AT指令后需调用本函数
 ******************************************************************************/
void at_clear_flag(void)
{
    at_flag = 0;
    at_state = AT_STATE_NONE;
    for(int i=0;i<4;i++) at_pwm_val[i]=0;
}
/****************************** at_clear_flag END ******************************/

/****************************** Fly_unlock BEGIN ******************************
 * 函    数：Fly_unlock
 * 功    能：解锁飞控系统，保持解锁状态
 * 参    数：无
 * 返 回 值：无
 * 注意事项：无
 ******************************************************************************/
void Fly_unlock(void)
{
    fly_pwm_set(0,2000,2000,2000,2000);
    rt_thread_delay(5000);
    fly_pwm_set(0,1000,1000,1000,1000);
    rt_thread_delay(3000);
    fly_unlock_flag = 1; // 解锁标志
}
/********************************Fly_unlock END ***************************/


/****************************** Fly_Stop BEGIN ********************************
 * 函    数：Fly_Stop
 * 功    能：停止飞控系统，关闭所有PWM输出
 * 参    数：无
 * 返 回 值：无
 * 注意事项：在停止前先设置所有PWM通道为最小值，延时2秒后再关闭所有PWM输出，确保飞控系统完全停止
 ******************************************************************************/
void Fly_Stop(void)
{
    fly_pwm_set(0,1000,1000,1000,1000); // 设置所有PWM通道为最小值
    rt_thread_delay(2000); // 延时2秒
    fly_pwm_set(0,0,0,0,0); // 关闭所有PWM输出
    rt_thread_delay(2000); // 延时2秒
}
 /******************************Fly_Stop END **********************************/

/****************************** at_get_user_ad BEGIN *****************************
 * 函    数：at_get_user_ad
 * 功    能：获取用户自定义的AD值
 * 参    数：idx —— AD通道索引（0~2）
 * 返 回 值：对应通道的AD值
 * 注意事项：idx超出范围时返回0
 ******************************************************************************/
int at_get_user_ad(int idx)
{
    if(idx < 0 || idx > 2) return 0;
    return user_ad[idx];
}
/****************************** at_get_user_ad END ********************************/

/****************************** at_get_user_key BEGIN *****************************
 * 函    数：at_get_user_key
 * 功    能：获取用户自定义的按键值
 * 参    数：无
 * 返 回 值：按键值
 * 注意事项：无
 ******************************************************************************/
int at_get_user_key(void)
{
    return user_key;
}
/****************************** at_get_user_key END ********************************/
